#!/usr/bin/python
from subprocess import call
import rospy
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import PoseStamped


global pub

def callback(data):
    global pub
    pose = PoseStamped()
    pose.pose = data.pose[3]
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "camera_init"
    pub.publish(pose)

if __name__ == "__main__":
    global pub
    rospy.init_node("model_state_to_pose")

    from_topic = rospy.get_param("from_topic", "/gazebo/model_states")
    to_topic = "pose"

    pub = rospy.Publisher(to_topic, PoseStamped, queue_size=10)
    
    rospy.Subscriber(from_topic, ModelStates, callback, queue_size=10)

    rospy.spin()